#ifndef SAVE_H
#define SAVE_H

#include <map>
#include <math.h>
#include <vector>
#include "dbg.h"

//#include <iostream>

template <class T>
inline T polint(
    const std::vector<T>& xa, const std::vector<T>& ya, T x, T *retdy=0)
{
    std::vector<T> c = ya;
    std::vector<T> d = ya;

    int n = xa.size();
    T dif=fabs(x-xa[0]);
    int ns = 0;
    for (int i=1;i<n;i++) {
        T dift = fabs(x-xa[i]);
        if (dift < dif) {
            ns=i;
            dif=dift;
        }
    }

    T y = ya[ns--];
    T dy=T(0);
    for (int m=1;m<n;m++) {
        for (int i=0;i<n-m;i++) {
            T ho=xa[i]-x;
            T hp=xa[i+m]-x;
            T w=c[i+1]-d[i];
            T den = ho-hp;
            Assert(den != 0.0);
            den=w/den;
            d[i]=hp*den;
            c[i]=ho*den;
        }
        dy=(2*ns < (n-m) ? c[ns+1] : d[ns--]);
        y += dy;
    }

    if (retdy) *retdy = dy;
    return y;
}

template <class T, class UF>
class SavedFunction : public std::unary_function<T, T>
{
protected:

    UF f;
    T acc;
    int order;
    mutable std::map<T,T> fmap;
    typedef typename std::map<T,T>::iterator mapit;

public:

    SavedFunction(const UF& _f, T _acc, int _order) : 
        f(_f), acc(_acc), order(_order) {}
    virtual ~SavedFunction() {}

    T operator()(T x) const
    {
        T y;
        if (InterpolateVal(x,&y)) return y; 
        else { return (fmap[x] = f(x)); }
    }

    bool InterpolateVal(T x, T* rety) const
    {
        mapit it = fmap.lower_bound(x);
        if (it == fmap.end()) return false;
        else if (x == it->first) { *rety = it->second; return true; }
        else if (it == fmap.begin()) return false;
        else if (order == 0) return false;
        else {
            //std::cout<<"need to actually interpolate"<<std::endl;

            Assert(x < it->first);
            mapit itx = it;
            int nlo = order/2+1; 
            for(int i=0;i<nlo;i++) {
                if (itx==fmap.begin()) return false;
                itx--;
                Assert(itx->first < x);
            }

            std::vector<T> vx,vy; 
            for(int i=0;i<=order;i++,itx++) {
                if (itx==fmap.end()) return false;
                vx.push_back(ConvertX(itx->first));
                vy.push_back(ConvertY(itx->second));
            }

            T dy;
            T y = polint(vx,vy,ConvertX(x),&dy);
            dy = fabs(dy);

            //std::cout <<"Interpolate: n = "<<vx.size()<<std::endl;
            //for(int i=0;i<vx.size();i++) {
            //  std::cout <<"f("<<vx[i]<<") = "<<vy[i]<<std::endl;
            //}
            //std::cout <<"==> f("<<ConvertX(x)<<") = "<<y<<" +- "<<dy<<std::endl;

            if (Success(y,dy)) {
                //std::cout <<"SUCCESSFUL INTERPOLATION"<<std::endl;
                //std::cout <<"dy = "<<dy<<", acc = "<<acc<<std::endl;
                *rety = UnConvertY(y);
                return true;
            } 
            else return false;
        }
    }

    virtual bool Success(T y, T dy) const = 0;
    virtual T ConvertX(T x) const = 0;
    virtual T ConvertY(T y) const = 0;
    virtual T UnConvertY(T y) const = 0;

};

template <class T, class UF>
class SavedFunctionLinear : public SavedFunction<T,UF>
{
public:

    SavedFunctionLinear(const UF& _f, T _acc=1.e-6, int _order=4) : 
        SavedFunction<T,UF>(_f,_acc,_order) {}
    virtual ~SavedFunctionLinear() {}

    virtual T ConvertX(T x) const { return x; }
    virtual T ConvertY(T y) const { return y; }
    virtual T UnConvertY(T y) const { return y; }
    virtual bool Success(T y, T dy) const { return dy < this->acc; }
};

template <class T, class UF>
class SavedFunctionLogLog : public SavedFunction<T,UF>
{
public:

    SavedFunctionLogLog(const UF& _f, T _acc=1.e-6, int _order=4) : 
        SavedFunction<T,UF>(_f,_acc,_order) {}
    virtual ~SavedFunctionLogLog() {}

    virtual T ConvertX(T x) const { return log(x); }
    virtual T ConvertY(T y) const { return log(y); }
    virtual T UnConvertY(T y) const { return exp(y); }
    virtual bool Success(T y, T dy) const { return dy < this->acc; }
    // dy here is really dlogy = dy/y, so this is relative error, not absolute
};

template <class T, class UF>
class SavedFunctionLog : public SavedFunction<T,UF>
{
public:

    SavedFunctionLog(const UF& _f, T _acc=1.e-6, int _order=4) : 
        SavedFunction<T,UF>(_f,_acc,_order) {}
    virtual ~SavedFunctionLog() {}

    virtual T ConvertX(T x) const { return log(x); }
    virtual T ConvertY(T y) const { return y; }
    virtual T UnConvertY(T y) const { return y; }
    virtual bool Success(T y, T dy) const { return dy < this->acc; }
};




#endif
